/***************************************************************************
 创建者: 华磊
 开始时间: 2019.9.20
 copyright: (C) 华友高科
 ***************************************************************************/
#include "pathplanmanager.h"
#include "mapmanager.h"
#include "fleettextcommandmaker.h"
#include <QDebug>
#include "domparser.h"


PathPlanManager::PathPlanManager(QString configFileIn)
{
     mapManager= MapManager::getInstance();
     //     fleetService=fleetServiceIn;
     lastPlanedStationId=-1;
     isErrorOccured=false;
     commandMaker=new FleetTextCommandMaker;
     isInitialOkStatus=false;

     if(1==initialConfigFile(configFileIn))
     {
         if(true==isUsePathPlan)
         {

             if(isUseDbMap)
             {

                 if(1!=mapManager->checkTable())
                 {
                     addMsg( ENUM_MSG_ERROR,  "PathPlanManager", "PathPlanManager",  12464,  -1);
                 }
             }

             QString mapconfigFile=D_TCRCONTROLLER_DATA_PATH;
             mapconfigFile+=D_ROS_CONFIG_FILE;
             mapconfigFile+="rosCommonConfig.xml";
             QString defaultMapPath;
             if(1!=readDefaultMapFilePath(mapconfigFile,defaultMapPath))
             {
                addMsg( ENUM_MSG_ERROR,  "PathPlanManager", "PathPlanManager",  12451,  -1);
             }
             else
             {
                 if(1==loadMap(defaultMapPath.toStdString()))
                 {
                     isInitialOkStatus=true;
                 }
             }
         }
         else
         {
             isInitialOkStatus=true;
         }

     }
     else
     {
         addMsg( ENUM_MSG_ERROR,  "PathPlanManager", "PathPlanManager",  12456,  -1);
     }




}

int PathPlanManager::getCurrentFloorId()
{
    return mapManager->getCurrentFloorId();
}

int PathPlanManager::getMapVersion()
{
    return mapManager->getMapVersion();
}

int PathPlanManager::getCurrentFleetMap(string &mapInfoReturn)
{
    return mapManager->getCurrentFleetMap(mapInfoReturn);
//    if(1!=tmpKey)
//    {
//        addMsg( ENUM_MSG_ERROR,  "PathPlanManager", "PathPlanManager",  12461,  -1,QString::fromStdString(mapName));
//    }
//    return tmpKey;
}

int PathPlanManager::getFleetMap(string mapName, string &mapInfoReturn)
{
    if(isUseDbMap)
    {
        return mapManager->getCurrentFleetMap(mapInfoReturn);
    }
    else
    {
        int tmpKey= mapManager->getFleetMap(getMapFleetFilePath(mapName),mapInfoReturn);
        if(1!=tmpKey)
        {
            addMsg( ENUM_MSG_ERROR,  "PathPlanManager", "PathPlanManager",  12461,  -1,QString::fromStdString(mapName));
        }
        return tmpKey;
    }
    return 1;

}

int PathPlanManager::writeFleetMap(string mapName, const  std::string &mapInfoIn)
{

    if(isUseDbMap)
    {
        mapManager->writeFleetMap_toDb(mapInfoIn);
        loadMap(mapName);
        addMsg( ENUM_MSG_REMIND,  "PathPlanManager", "PathPlanManager",  12460,  -1,QString::fromStdString(mapName)
                ,mapManager->getMapVersion());
    }
    else
    {
        mapManager->writeFleetMap(getMapFleetFilePath(mapName),mapInfoIn);
        loadMap(mapName);
        addMsg( ENUM_MSG_REMIND,  "PathPlanManager", "PathPlanManager",  12460,  -1,QString::fromStdString(mapName)
                ,mapManager->getMapVersion());
    }
    return 1;

}

int PathPlanManager::loadMap(string mapName)
{
    if(false==isUsePathPlan)
    {
        qDebug()<<"no use PathPlanManager";
        return 0;
    }
    if(isUseDbMap)
    {
        qDebug()<<"PathPlanManager::loadMap from db";
        int tmpKey=mapManager->loadMapFromDB();
        switch (tmpKey) {
        case 1:
            return 1;
            break;
        case -1:
            addMsg( ENUM_MSG_WARNING,  "PathPlanManager", "PathPlanManager",  12452,  -1,QString::fromStdString(mapName));
            break;
        default:
            addMsg( ENUM_MSG_ERROR,  "PathPlanManager", "PathPlanManager",  12455,  -1,QString::fromStdString(mapName));
            break;
        }
        return 1;

    }
    else
    {
        //    errorOccured();修改点位会很麻烦
            qDebug()<<"PathPlanManager::loadMap"<<QString::fromStdString(mapName);
            int tmpKey=mapManager->loadMap(getMapFleetFilePath(mapName));
            switch (tmpKey) {
            case 1:
                return 1;
                break;
            case -1:
                addMsg( ENUM_MSG_WARNING,  "PathPlanManager", "PathPlanManager",  12452,  -1,QString::fromStdString(mapName));
                break;
            case -2:
                addMsg( ENUM_MSG_ERROR,  "PathPlanManager", "PathPlanManager",  12453,  -1,QString::fromStdString(mapName));
                break;
            case -3:
                addMsg( ENUM_MSG_ERROR,  "PathPlanManager", "PathPlanManager",  12454,  -1,QString::fromStdString(mapName));
                break;
            default:
                addMsg( ENUM_MSG_ERROR,  "PathPlanManager", "PathPlanManager",  12455,  -1,QString::fromStdString(mapName));
                break;
            }
            return 1;

    }

    return 1;
}

int PathPlanManager::setPositionFeedback(PositionInfo_hmi amcl, PositionInfo_hmi vmark,
                                         PositionInfo_hmi qrcode, PositionInfo_hmi reflect)
{
    realtime_position_amcl=amcl;
    realtime_position_vmark=vmark;
    realtime_position_qrcode=qrcode;
    realtime_position_reflect=reflect;
    return 1;
}

int PathPlanManager::setStationPathStatus(PathMovingInfo pathMovingInfoIn)
{
    pathMovingInfo=pathMovingInfoIn;
    return 1;
}



int PathPlanManager::getBestPath_fromStationId( int startStationId,
                                 int targetStationId, QStringList &pathCommandOut, int &tmpNearstOut)
{
    //样条曲线的开始点，强制增加一个旋转，使车头方向和路径方向平行。
    int tmpKey;
    pathCommandOut.clear();
    //宏友要求报错保留站点信息　isErrorOccured ||. 报错时，怎么知道前面规划的站点是否都执行完成。
    if( startStationId<0 )//有报错或从未规划过
    {
        //找最近的站点//添加一条直线路径。用样条曲线会导致路径变小，在站点附近由于角度误差会变形严重。
        QString tmpPrepareMove;
        int tmpNearstStationId;

        tmpKey=moveToNearstReportStation(defaultPositionType,tmpPrepareMove,tmpNearstStationId);
        if(-100>=tmpKey)
        {
            tmpNearstOut=-1;
            qDebug()<<"error,,,,找不到最近站点1";
            addMsg( ENUM_MSG_ERROR,  "PathPlanManager", "PathPlanManager",  12459,  -1);
            return -100;
        }
        else if(0>tmpKey)
        {
            tmpNearstOut=-2;
            qDebug()<<"error,,,,找不到最近站点2";
            addMsg( ENUM_MSG_ERROR,  "PathPlanManager", "PathPlanManager",  12458,  -1);
            return -101;
        }
        tmpNearstOut=tmpNearstStationId;



        //添加剩余路径
        tmpKey= getPathCommandString(tmpNearstStationId,targetStationId,pathCommandOut);
//        if(tmpNearstStationId!=targetStationId)//不要判断。
//        {
             pathCommandOut.push_front(tmpPrepareMove);
//        }


    }
    else//有规划过,且无错误
    {
        tmpNearstOut=0;
        tmpKey= getPathCommandString(startStationId,targetStationId,pathCommandOut);
    }
    if(1==tmpKey)
    {
//        lastPlanedStationId=targetStationId;
        isErrorOccured=false;
    }
    else if(-201>=tmpKey)
    {
        qDebug()<<"error,,,,地图路径内部错误,-201>=tmpKey";
        addMsg( ENUM_MSG_ERROR,  "PathPlanManager", "PathPlanManager",  12462,  -1);
        return tmpKey;
    }


    return tmpKey;
}

int PathPlanManager::getBestPath_byPathInfo(PathMovingInfo pathMovingInfoIn,
                                    int targetStationId, QStringList &pathCommandOut, int &tmpNearstOut,int &methodOut)
{
    if(pathMovingInfoIn.currentStationId>0)//在站点上面
    {
        methodOut=0;
        return getBestPath_fromStationId(pathMovingInfoIn.currentStationId,targetStationId,pathCommandOut,tmpNearstOut);
    }
    else//不在在站点上面
    {
        if(pathMovingInfoIn.currentPathId>0)//有当前路径
        {
//            isInPathRegion(pathMovingInfoIn.currentPathId,);
//            //没有超出路径范围
//            methodOut=2;
            MapPath *tmpPath=mapManager->getPathById(pathMovingInfoIn.currentPathId);
            MapPoint *tmpEndP;
            if(nullptr==tmpPath)
            {
                qDebug()<<"error11,getCommandStringByPathId,nullptr==tmpPath";
                return -103;
            }
            tmpEndP=mapManager->getPointById(tmpPath->getEnd());
            if(nullptr==tmpEndP )
            {
                qDebug()<<"error22,getCommandStringByPathId,nullptr==tmpEndP";
                return -104;
            }
            if(MapPoint::Map_Point_Type_Draw!=tmpEndP->getPointType()
                    && MapPoint::Map_Point_Type_REPORT!=tmpEndP->getPointType()
                    && MapPoint::Map_Point_Type_HALT!=tmpEndP->getPointType())
            {
                qDebug()<<"error22,getCommandStringByPathId,MapPoint::Map_Point_Type_Draw!=tmpEndP->getPointType()";
                return -105;
            }
            return getBestPath_fromPathId(pathMovingInfoIn.currentPathId,targetStationId,pathCommandOut);

        }
        else//无当前路径
        {
            methodOut=1;
            return getBestPath_fromStationId(-1,targetStationId,pathCommandOut,tmpNearstOut);
        }
    }
}

int PathPlanManager::errorOccured()
{
    isErrorOccured=true;
}

int PathPlanManager::getBestPath_fromPathId(int pathId, int targetStationId, QStringList &pathCommandOut)
{
    int tmpKey;
    pathCommandOut.clear();

    //添加第一条路径
    QString tmpPrepareMove;
    if(1==getCommandStringByPathId(pathId,false,true,tmpPrepareMove))
    {

    }
    else
    {
        qDebug()<<"error,,,getBestPath_fromPathId,getCommandStringByPathId"<<pathId;
        return -2;
    }


    //添加剩余路径
     MapPath *tmpPath=mapManager->getPathById(pathId);
    tmpKey= getPathCommandString(tmpPath->getEnd(),targetStationId,pathCommandOut);
    pathCommandOut.push_front(tmpPrepareMove);

    if(1==tmpKey)
    {
        isErrorOccured=false;
    }
    else if(-201>=tmpKey)
    {
        qDebug()<<"error,,,,地图路径内部错误,-201>=tmpKey";
        addMsg( ENUM_MSG_ERROR,  "PathPlanManager", "PathPlanManager",  12462,  -1);
        return tmpKey;
    }


    return tmpKey;
}



int PathPlanManager::isInPathRegion(int pathId, PositionInfo_hmi rosPose, PositionInfo_hmi reflectPose,
                                    PositionInfo_hmi qrcodePose, PositionInfo_hmi vmarkPose,double allowDistanceOffset,
                                    double allowAngleOffset)
{

}

string PathPlanManager::getMapFleetFilePath(string mapNameIn)
{
    QString  mapName=QString::fromStdString(mapNameIn);

    QRegExp rx("(.*).yaml");

    bool isMatch=rx.exactMatch(mapName);

    if(false==isMatch )
    {
        addMsg( ENUM_MSG_ERROR,  "PathPlanManager", "PathPlanManager",  12457,  -1,QString::fromStdString(mapNameIn));
        qDebug()<<"PathPlanManager　，map file name error "<<mapName;
        return "";
    }

    QString mapOriginalName=rx.cap(1);

    QString   filePath = D_MAP_PATH + mapOriginalName+".mjson";
    return filePath.toStdString();
}

int PathPlanManager::getPathCommandString(int startStation, int targetStationId,
                                          QStringList &pathCommandOut)
{
    pathCommandOut.clear();
    if(startStation==targetStationId)
    {
        return 1;
    }
    int agvid=1;
    int lastStationId=startStation;
    bool isReverse=false;
    int tmpLength;
//    std::vector<int> tmpPathList=mapManager->getBestPath(agvid,lastStationId, startStation, targetStationId,
//                                 tmpLength, isReverse);
    int isOkOut;
    std::vector<int> tmpPathList=mapManager->getPath_byStation(startStation, targetStationId,
                                 tmpLength,isOkOut, isReverse);
    if(1!=isOkOut)
    {
        qDebug()<<"failed,getPathCommandString,1!=isOkOut"<<isOkOut;
        return -201;
    }
    else
    {
        if(0==tmpPathList.size())
        {
            qDebug()<<"failed,getPathCommandString,no valid path found,startStation"
                   <<startStation<<"targetStationId"<<targetStationId;
            return -1;
        }
    }


    bool isCntControl;
    bool isCntControl_old;
    //增加剩余路径
    for(int i=0;i<tmpPathList.size();i++)
    {
        isCntControl=true;
        if(i==tmpPathList.size()-1)
        {
            isCntControl=false;
        }
        else
        {
            //后一条
            MapPath *tmpNextPath=mapManager->getPathById(tmpPathList[i+1]);
            if(MapPath::Map_Path_Type_Cubic_Bezier!=tmpNextPath->getPathType())
            {
                isCntControl=false;
            }

            MapPoint *tmpStartP=mapManager->getPointById(tmpNextPath->getStart());
            MapPoint *tmpEndP=mapManager->getPointById(tmpNextPath->getEnd());
            if(tmpStartP->getPoseType()!=tmpEndP->getPoseType())
            {
                isCntControl=false;
            }
            //当前条
            MapPath *tmpCurrentPath=mapManager->getPathById(tmpPathList[i]);
            if( ( MapPath::Map_Path_Type_Cubic_Bezier!=tmpCurrentPath->getPathType() &&
                    MapPath::Map_Path_Type_Cubic_Bezier==tmpNextPath->getPathType() )
                    ||  tmpCurrentPath->getIsBackMove()!=tmpNextPath->getIsBackMove()
                    || tmpCurrentPath->getStart()==tmpNextPath->getEnd())
            {
                isCntControl=false;

            }

            //前后两条样条曲线的过渡角度偏差大于设定值
            if( ( MapPath::Map_Path_Type_Cubic_Bezier==tmpCurrentPath->getPathType() &&
                    MapPath::Map_Path_Type_Cubic_Bezier==tmpNextPath->getPathType() ))
            {
                if(getTransAngleBiasOfSplines(tmpPathList[i],tmpPathList[i+1])>cntAngleBias)
                {
                    isCntControl=false;
                }


            }
        }

        //如果第一路径是样条曲线而且首尾点都是amcl定位,或都是reflect反射板定位，
        //而且最后规划姿态与曲线开始角度偏差超过３度，则增加一个原地旋转车头到与曲线开始角度平行。
        if(0==i || false==isCntControl_old)
        {
            //第一条 或　上一条非平滑控制。todo 转角准备处理
            QString prepareCommand;
            int tmpKey=getRotatePrepareCommandByPathId(tmpPathList[i],prepareCommand);
            if(1==tmpKey)
            {
                pathCommandOut.append(prepareCommand);
            }
            else if(-1000==tmpKey)
            {
                return -1000;
            }
            else
            {

            }
        }


        QString tmpCommand;
        if(1==getCommandStringByPathId(tmpPathList[i],isCntControl,false,tmpCommand))
        {
              pathCommandOut.append(tmpCommand);

        }
        else
        {
            qDebug()<<"error,,,getCommandStringByPathId";
            return -2;
        }

        //增加站点动作文本指令
        MapPath *tmpCurrentPath=mapManager->getPathById(tmpPathList[i]);
        MapPoint *tmpStation=mapManager->getPointById(tmpCurrentPath->getEnd());
        std::string tmpAction=tmpStation->getActionCommand();
        if(0!=tmpAction.size())
        {
            QString tmpActionString="\n";
            tmpActionString+=QString::fromStdString(tmpAction)+"\r";
            pathCommandOut.append(tmpActionString);
        }


        isCntControl_old=isCntControl;

    }
    return 1;

}

int PathPlanManager::moveToNearstReportStation(E_PLAN_POSITION_TYPE poseType,QString &pathCommandOut,
                                                            int &nearestIdout)
{
    E_POSITION_TYPE tmpPositionType=(E_POSITION_TYPE)poseType;
    double tmpX,tmpY;
    MOVE_FREE_POINT_TYPE tmpMoveType;
    switch (tmpPositionType) {
    case E_POSITION_TYPE_COMMON:
        if(false==realtime_position_amcl.isUpdateSucess)
        {
            qDebug()<<"定位不成功，无法找到最近站点a";
            return -100;
        }
        tmpX=realtime_position_amcl.position.x;
        tmpY=realtime_position_amcl.position.y;
        tmpMoveType=EM_MOVE_FREE_DP;
        break;
    case E_POSITION_TYPE_VMARK:
        if(false==realtime_position_vmark.isUpdateSucess)
        {
            qDebug()<<"定位不成功，无法找到最近站点b";
            return -101;
        }
        tmpX=realtime_position_vmark.position.x;
        tmpY=realtime_position_vmark.position.y;
        tmpMoveType=EM_MOVE_FREE_VMARK;
        break;
    case E_POSITION_TYPE_QRCODE:
        if(false==realtime_position_qrcode.isUpdateSucess)
        {
            qDebug()<<"定位不成功，无法找到最近站点c";
            return -102;
        }
        tmpX=realtime_position_qrcode.position.x;
        tmpY=realtime_position_qrcode.position.y;
        tmpMoveType=EM_MOVE_FREE_QR;
        break;
    case E_POSITION_TYPE_REFLECT:
        if(false==realtime_position_reflect.isUpdateSucess)
        {
            qDebug()<<"定位不成功，无法找到最近站点d";
            return -103;
        }
        tmpX=realtime_position_reflect.position.x;
        tmpY=realtime_position_reflect.position.y;
        tmpMoveType=EM_MOVE_FREE_CU1;
        break;
    default:
        qDebug()<<"未知定位不成功，无法找到最近站点unknow";
        return -104;
        break;
    }

    int tmpKey=mapManager->getNearestReportStation(tmpPositionType,tmpX,tmpY ,nearestIdout);
    if(1!=tmpKey)
    {
        qDebug()<<"error，无法找到最近站点";
        return -2;
    }
    MapPoint *tmpPoint=mapManager->getPointById(nearestIdout);
    LaserPathParameter paraIn;
    paraIn.stationId=nearestIdout;
    paraIn.pathId=-1000;
    paraIn.isPositiveDirection=true;
    paraIn.pathControlStrategy=E_PATH_CONTROL_STRATEGY_DIFFERENTIAL;
    paraIn.CNT_Value=0;
    paraIn.targetPosition.x=tmpPoint->getRealX();
    paraIn.targetPosition.y=tmpPoint->getRealY();
    paraIn.targetPosition.rotate=0;
    paraIn.pointSensorType=tmpMoveType;      // 自由导航指令点位模式
    paraIn.pointIndex=0;//点位类型索引,vmark类型
    paraIn.laserTypeNum=0;
    paraIn.velocityRatio=30;
    paraIn.isDetectBarrier=true; //是否探测障碍物
    paraIn.barrierRunMode=1;//窄通道．普通模式很容易壁障无法运行．
    paraIn.allowAccuracy=0.25;//停车精度，单位米
    paraIn.moveFreeMoveTrajectoryType=E_MOVE_FREE_MOVE_TRAJECTORY_LINE;   // 自由导航指令运行轨迹类型；  AUTO:自由轨迹  LINE:直线轨迹  SPLINE:样条轨迹

    //默认
    paraIn.moveFreeMode=E_MOVE_FREE_IMU;                 // 自由导航模式
    paraIn.pointArryType=E_MOVE_FREE_POINTS_ARRY_DP;    // 自由导航点位类型
    paraIn.pointsValue=0;                            // DP单点数值
    paraIn.registerPointsValue=0;                    // DPR寄存器点数值


    return commandMaker->makeAgvMotionLaserPath(paraIn,pathCommandOut);

}

int PathPlanManager::moveToPathEnd(int pathId, QString &pathCommandOut)
{

}

int PathPlanManager::makeTransferStationPathCommand(int stationIdIn, QString &pathCommandOut)
{

}

int PathPlanManager::getCommandStringByPathId(int pathId,bool isCnt,bool isForceLinePath,
                                              QString &pathCommandOut)
{
    //todo 转角准备处理
    pathCommandOut.clear();
    MapPath *tmpPath=mapManager->getPathById(pathId);
    MapPoint *tmpEndP;
    if(nullptr==tmpPath)
    {
        qDebug()<<"error,getCommandStringByPathId,nullptr==tmpPath";
        return -100;
    }
    tmpEndP=mapManager->getPointById(tmpPath->getEnd());
    if(nullptr==tmpEndP )
    {
        qDebug()<<"error,getCommandStringByPathId,nullptr==tmpEndP";
        return -101;
    }
    LaserPathParameter paraIn;
    paraIn.stationId=tmpPath->getEnd();
    paraIn.pathId=pathId;
    paraIn.pathControlStrategy=E_PATH_CONTROL_STRATEGY_DIFFERENTIAL;
    if(isCnt)
    {
            paraIn.CNT_Value=100;
    }
    else
    {
            paraIn.CNT_Value=0;
    }

    paraIn.targetPosition.x=tmpEndP->getRealX();
    paraIn.targetPosition.y=tmpEndP->getRealY();

    if(tmpPath->getIsBackMove())
    {
         paraIn.isPositiveDirection=false;
    }
    else
    {
        paraIn.isPositiveDirection=true;
    }
    switch (tmpEndP->getPoseType()) {
    case E_POSITION_TYPE_COMMON:
        paraIn.pointSensorType=EM_MOVE_FREE_DP;      // 自由导航指令点位模式
        break;
    case E_POSITION_TYPE_VMARK:
        paraIn.pointSensorType=EM_MOVE_FREE_VMARK;      // 自由导航指令点位模式
        break;
    case E_POSITION_TYPE_REFLECT:
        paraIn.pointSensorType=EM_MOVE_FREE_CU1;      // 自由导航指令点位模式
        break;
    case E_POSITION_TYPE_QRCODE:
        paraIn.pointSensorType=EM_MOVE_FREE_QR;      // 自由导航指令点位模式
        break;
    default:
        break;
    }

    paraIn.pointIndex=0;//点位类型索引,vmark类型
    paraIn.laserTypeNum=0;
    paraIn.velocityRatio=100*tmpPath->getSpeed();

    if(E_PATH_BARRIER_TYPE_NODETECT==tmpPath->getBarrierType())
    {
      paraIn.isDetectBarrier=false; //是否探测障碍物
      paraIn.barrierRunMode=0;
    }
    else
    {
        paraIn.isDetectBarrier=true; //是否探测障碍物
        paraIn.barrierRunMode=tmpPath->getBarrierType()-1;
    }

    paraIn.allowAccuracy=0.25;//停车精度，单位米
    if(isForceLinePath)
    {
        paraIn.moveFreeMoveTrajectoryType=E_MOVE_FREE_MOVE_TRAJECTORY_LINE;
        paraIn.CNT_Value=0;
        paraIn.targetPosition.rotate=M_PI*tmpEndP->getRealA()/180.0;
    }
    else
    {
        switch (tmpPath->getPathType()) {
        case MapPath::Map_Path_Type_Line:
            paraIn.moveFreeMoveTrajectoryType=E_MOVE_FREE_MOVE_TRAJECTORY_LINE;
            paraIn.CNT_Value=0;
            paraIn.targetPosition.rotate=M_PI*tmpEndP->getRealA()/180.0;
            break;
        case MapPath::Map_Path_Type_Quadratic_Bezier:
            qDebug()<<"不支持二次曲线";
            return -1;
            break;
        case MapPath::Map_Path_Type_Cubic_Bezier:
            paraIn.moveFreeMoveTrajectoryType=E_MOVE_FREE_MOVE_TRAJECTORY_SPLINE;
                if (E_POSITION_TYPE_VMARK==tmpEndP->getPoseType())
                {
                    paraIn.targetPosition.rotate=M_PI*tmpEndP->getRealA()/180.0;
                }
                else
                {
                    double tmpEndAngle;
                    mapManager->getSplineEndAngle(pathId,tmpEndAngle);
                    paraIn.targetPosition.rotate=M_PI*tmpEndAngle/180.0;
                    if(tmpPath->getIsBackMove())
                    {
                         paraIn.targetPosition.rotate+=M_PI;
                    }

                }

            break;
        default:
            break;
        }
    }


    //默认
    paraIn.moveFreeMode=E_MOVE_FREE_IMU;                 // 自由导航模式
    paraIn.pointArryType=E_MOVE_FREE_POINTS_ARRY_DP;    // 自由导航点位类型
    paraIn.pointsValue=0;                            // DP单点数值
    paraIn.registerPointsValue=0;                    // DPR寄存器点数值

    //todo 转角准备处理
//    if (E_POSITION_TYPE_VMARK==tmpEndP->getPoseType())
//    {
//        paraIn.isRotatePrepare=false;
//    }
//    else
//    {
//        paraIn.isRotatePrepare=true;
//        double tmpStartAngle;
//        mapManager->getSplineStartAngle(pathId,tmpStartAngle);
//        paraIn.prepareAngle=M_PI*tmpStartAngle/180.0;
//        if(tmpPath->getIsBackMove())
//        {
//             paraIn.prepareAngle+=M_PI;
//        }

//    }
    mapManager->getSplineMiddlePosition(pathId,paraIn.p1x,paraIn.p1y,paraIn.p2x,paraIn.p2y);

    return commandMaker->makeAgvMotionLaserPath(paraIn,pathCommandOut);
}

double PathPlanManager::getTransAngleBiasOfSplines(int firstPathId,int secondPathId)
{
    double angle1,angle2;
    mapManager->getSplineEndAngle(firstPathId,angle1);
    mapManager->getSplineStartAngle(secondPathId,angle2);
    double angleBias=angleRotationJudgement(angle1/180.0*M_PI,angle2/180.0*M_PI);
    angleBias=180.0*fabs(angleBias)/M_PI;
    return angleBias;
}

int PathPlanManager::getRotatePrepareCommandByPathId(int pathId, QString &pathCommandOut)
{
    //如果第一路径是样条曲线而且首尾点都是amcl定位,或都是reflect反射板定位，
    //而且最后规划姿态与曲线开始角度偏差超过３度，则增加一个原地旋转车头到与曲线开始角度平行。

    MapPath *tmpPath=mapManager->getPathById(pathId);
    if(MapPath::Map_Path_Type_Line==tmpPath->getPathType()
            ||MapPath::Map_Path_Type_Between_Floor==tmpPath->getPathType())
    {
        qDebug()<<"不需要做旋转准备,路径类型不符合"<<tmpPath->getPathType();
        return 0;
    }

    MapPoint *tmpStartP=mapManager->getPointById(tmpPath->getStart());
    MapPoint *tmpEndP=mapManager->getPointById(tmpPath->getEnd());
    bool isPrepareRotate=false;

    //
    if(MapPath::Map_Path_Type_Cubic_Bezier==tmpPath->getPathType())
    {
        if( (E_POSITION_TYPE_COMMON==tmpStartP->getPoseType()
                    &&E_POSITION_TYPE_REFLECT==tmpEndP->getPoseType())
                || (E_POSITION_TYPE_REFLECT==tmpStartP->getPoseType()
                    &&E_POSITION_TYPE_COMMON==tmpEndP->getPoseType()))
            {
                qDebug()<<"error,首尾点类型不一样，不允许样条曲线运行";
                addMsg( ENUM_MSG_ERROR,  "PathPlanManager", "PathPlanManager",  12463,  -1);
                return -1000;
            }

    }

    //点类型判断
    if(E_POSITION_TYPE_COMMON==tmpStartP->getPoseType()
            &&E_POSITION_TYPE_COMMON==tmpEndP->getPoseType())
    {
        isPrepareRotate=true;
    }
    else if(E_POSITION_TYPE_REFLECT==tmpStartP->getPoseType()
            &&E_POSITION_TYPE_REFLECT==tmpEndP->getPoseType())
    {
        isPrepareRotate=true;
    }
    else if(E_POSITION_TYPE_QRCODE==tmpStartP->getPoseType()
            &&E_POSITION_TYPE_QRCODE==tmpEndP->getPoseType())
    {
        isPrepareRotate=true;
    }
    else
    {
        qDebug()<<"不需要做旋转准备,路径开始结束点类型不符合要求"<<tmpStartP->getPoseType()<<tmpEndP->getPoseType();
        return 0;
    }

    //角度偏差判断.最后生成的角度和曲线开始角度
    double startAngle;
    mapManager->getSplineStartAngle(pathId,startAngle);
    //need todo.需要判断最后生成的什么类型点，而且要记录lastPlaned_position_amcl
    qDebug()<<"添加旋转准备...need to do ,增加角度判别是否需要";

    LaserPathParameter paraIn;
    paraIn.stationId=tmpPath->getStart();
    paraIn.pathId=pathId;
    paraIn.isPositiveDirection=true;
    paraIn.pathControlStrategy=E_PATH_CONTROL_STRATEGY_DIFFERENTIAL;
    paraIn.CNT_Value=0;
    paraIn.targetPosition.x=tmpStartP->getRealX();
    paraIn.targetPosition.y=tmpStartP->getRealY();
//    paraIn.targetPosition.rotate=M_PI*tmpStartP->getRealA()/180.0;
    paraIn.targetPosition.rotate=M_PI*startAngle/180.0;
    if(tmpPath->getIsBackMove())
    {
        paraIn.targetPosition.rotate+=M_PI;//退着运行需要姿态加１８０
    }
    switch(tmpStartP->getPoseType())
    {
    case E_POSITION_TYPE_COMMON:
    {
        paraIn.pointSensorType=EM_MOVE_FREE_DP;      // 自由导航指令点位模式
        break;
    }
    case E_POSITION_TYPE_VMARK:
    {
        paraIn.pointSensorType=EM_MOVE_FREE_VMARK;      // 自由导航指令点位模式
        break;
    }
    case E_POSITION_TYPE_QRCODE:
    {
        paraIn.pointSensorType=EM_MOVE_FREE_QR;      // 自由导航指令点位模式
        break;
    }
    case E_POSITION_TYPE_REFLECT:
    {
        paraIn.pointSensorType=EM_MOVE_FREE_CU1;      // 自由导航指令点位模式
        break;
    }
    default:
    {
        return -1;
    }
    }

    paraIn.pointIndex=0;//点位类型索引,vmark类型
    paraIn.laserTypeNum=0;
    paraIn.velocityRatio=30;

    if(E_PATH_BARRIER_TYPE_NODETECT==tmpPath->getBarrierType())
    {
      paraIn.isDetectBarrier=false; //是否探测障碍物
      paraIn.barrierRunMode=0;
    }
    else
    {
        paraIn.isDetectBarrier=true; //是否探测障碍物
        paraIn.barrierRunMode=tmpPath->getBarrierType()-1;
    }

    paraIn.allowAccuracy=0.25;//停车精度，单位米
    paraIn.moveFreeMoveTrajectoryType=E_MOVE_FREE_MOVE_TRAJECTORY_ROTATE;   // 自由导航指令运行轨迹类型；  AUTO:自由轨迹  LINE:直线轨迹  SPLINE:样条轨迹

    //默认
    paraIn.moveFreeMode=E_MOVE_FREE_IMU;                 // 自由导航模式
    paraIn.pointArryType=E_MOVE_FREE_POINTS_ARRY_DP;    // 自由导航点位类型
    paraIn.pointsValue=0;                            // DP单点数值
    paraIn.registerPointsValue=0;                    // DPR寄存器点数值


    return commandMaker->makeAgvMotionLaserPath(paraIn,pathCommandOut);


}


int PathPlanManager::initialConfigFile(QString configFileName)
{
    qDebug() << "PathPlanManager::initialConfigFile ==== 初始化配置文件：" << configFileName;

    DomParser domparser;
    bool ok = domparser.openXml(configFileName, QIODevice::ReadOnly);
    if( !ok )
    {
        qDebug() << "PathPlanManager::initialConfigFile ==== 配置文件打开失败：" << configFileName;
        return -1;
    }

    QDomNode domNode = domparser.findSubNode(domparser.getRootDomElement(), "isUsePathPlan", ok );
    if( !ok )
    {
        qDebug() << "PathPlanManager::initialConfigFile failed" << configFileName;
        return -1;
    }
    domparser.readXml(domNode, isUsePathPlan);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "pathPlanStratage", ok );
    if( !ok )
    {
        qDebug() << "PathPlanManager::initialConfigFile failed" << configFileName;
        return -1;
    }
    domparser.readXml(domNode, pathPlanStratage);

    int tmpType;
    domNode = domparser.findSubNode(domparser.getRootDomElement(), "defaultPositionType", ok );
    if( !ok )
    {
        qDebug() << "PathPlanManager::initialConfigFile failed" << configFileName;
        return -1;
    }
    domparser.readXml(domNode, tmpType);
    defaultPositionType=(E_PLAN_POSITION_TYPE)tmpType;

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "isUseDbMap", ok );
    if( !ok )
    {
        qDebug() << "PathPlanManager::initialConfigFile  isUseDbMap failed" << configFileName;
        return -1;
    }
    domparser.readXml(domNode, isUseDbMap);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "cntAngleBias", ok );
    if( !ok )
    {
        qDebug() << "PathPlanManager::initialConfigFile  cntAngleBias failed" << configFileName;
        return -1;
    }
    domparser.readXml(domNode, cntAngleBias);


    domparser.closeXml();
    return 1;
}

double PathPlanManager::angleRotationJudgement(double targetAngle, double initialAngle)
{
    double angle1,angle2,angle3;
    angle1=targetAngle-initialAngle;
    angle2=angle1+2*M_PI;
    angle3=angle1-2*M_PI;
    double minNum;
    if(fabs(angle1)<=fabs(angle2))
    {
        minNum=angle1;
    }
    else
    {
        minNum=angle2;
    }

    if(fabs(angle3)<=fabs(minNum))
    {
        minNum=angle3;
    }

    return minNum;
}


int PathPlanManager::readDefaultMapFilePath(QString configFilePathIn,QString &defaultMapPathOut)
{
    DomParser xmlParser;

    bool ok = xmlParser.openXml(configFilePathIn, QIODevice::ReadOnly );
    if( !ok )//无法打开文件！
    {
       qDebug()<<"error, loadDefaultNavagationParameter open file error！";
       return -1;
    }

    QDomNode  inNode = xmlParser.findSubNode(xmlParser.getRootDomElement(), "CommSetting", ok );
    if( !ok )
    {

        qDebug()<<"error, loadDefaultNavagationParameter node error！";
        return -2;
    }

    //节点ｒｅａｄ数据
    xmlParser.readXml( xmlParser.findSubNode(inNode, "defaultMapFileName", ok), defaultMapPathOut  );
    if( !ok )
    {
        qDebug()<<"error, setDefaultMapName node error！";
        return -4;
    }
    qDebug()<<"defaultMapPathOut="<<defaultMapPathOut;


    xmlParser.closeXml( );

    return 1;
}

void PathPlanManager::addMsg(int messageLevel, string componentName, string messageType, int messageCode,
                             int robotId, QString parameter1, int parameter2, int parameter3, int parameter4)
{
    Message tmpMsg;
    tmpMsg.MessageLevel = messageLevel;
    tmpMsg.componentClassName = componentName;
    tmpMsg.messageType = messageType;
    tmpMsg.robotId = robotId;
    tmpMsg.messageCode = messageCode;


    QString infomationStr;

    switch(messageCode)
    {
    case 12464:
    {
        infomationStr =  QObject::tr("地图数据库加载失败！")
                .arg(parameter1).arg(parameter2);
        break;
    }
    case 12463:
    {
        infomationStr =  QObject::tr("首尾点类型不一样，不允许样条曲线运行！")
                .arg(parameter1).arg(parameter2);
        break;
    }
    case 12462:
    {
        infomationStr =  QObject::tr("路径规划模块:地图路径内部错误！")
                .arg(parameter1).arg(parameter2);
        break;
    }
    case 12461:
    {
        infomationStr =  QObject::tr("路径规划模块:地图(%1)获取失败！")
                .arg(parameter1).arg(parameter2);
        break;
    }
    case 12460:
    {
        infomationStr =  QObject::tr("路径规划模块:地图(%1)已经重新加载，版本(%2)！")
                .arg(parameter1).arg(parameter2);
        break;
    }
    case 12459:
    {
        infomationStr =  QObject::tr("路径规划模块:缺省定位方式处于失败状态，无法找最近过渡站点！")
                .arg(parameter1);
        break;
    }
    case 12458:
    {
        infomationStr =  QObject::tr("路径规划模块:找不到最近的过渡站点！")
                .arg(parameter1);
        break;
    }
    case 12457:
    {
        infomationStr =  QObject::tr("路径规划模块:加载地图(%1)失败，地图名称非yaml格式！")
                .arg(parameter1);
        break;
    }
    case 12456:
    {
        infomationStr =  QObject::tr("路径规划模块:加载配置文件(pathPlanConfig.xml)失败！")
                .arg(parameter1);
        break;
    }
    case 12455:
    {
        infomationStr =  QObject::tr("路径规划模块:加载地图(%1)失败，未知错误！")
                .arg(parameter1);
        break;
    }
    case 12454:
    {
        infomationStr =  QObject::tr("路径规划模块加载地图(%1)失败，json格式错误！")
                .arg(parameter1);
        break;
    }
    case 12453:
    {
        infomationStr =  QObject::tr("路径规划模块:加载地图(%1)失败，mjson文件打开失败！")
                .arg(parameter1);
        break;
    }
    case 12452:
    {
        infomationStr =  QObject::tr("路径规划模块:加载地图(%1)失败,mjson文件不存在！")
                .arg(parameter1);
        break;
    }
    case 12451:
    {
        infomationStr =  QObject::tr("路径规划模块:读取默认地图配置文件失败！")
                .arg(parameter1);
        break;
    }
    default:

        break;
    }


    tmpMsg.MessageInformation = infomationStr.toStdString();

    MessageLog::getInstance()->addMessage(tmpMsg);
}
